/* gm_monitor.c -- Monitora a posição dos objetos em campo
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 *
 * This file is part os GEARMedula.
 * 
 * GEARMedula is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, version 3 of the
 * License.
 * 
 * GEARMedula is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

#include <pthread.h>
#include <iostream>

#include "GEARMedulaIDL.hh"
#include "Monitor.hh"

using namespace std;

Monitor::Monitor()
{
    int i = 0;
    /* Iniciando o mundo */
    world.resize( LENGTH_WORLD_VECTOR, 0.0 );
    pthread_mutex_init( &(world_mutex),
			NULL );

    /* Iniciando field */
    field.resize( LENGTH_FIELD_VECTOR, 0.0 );
    pthread_mutex_init( &(field_mutex),
			NULL );
}

Monitor::~Monitor()
{
    // if( self->f_display )
    // 	gm_monitor_display_enable( self, 0 );
}

void
Monitor::Init( const int f_real )
{
    if( f_real )
	f_real_ = 1;
    else
	f_real_ = 0;
}

void
Monitor::setSim( GMSim * sim )
{
    sim_ = sim;
}

void
Monitor::setWorld( const vector<float> world_vec )
{
    pthread_mutex_lock( &(world_mutex) );

    world = world_vec;

    pthread_mutex_unlock( &(world_mutex) );
}

void
Monitor::getWorld( vector<float> &world_vec )
{
    vector<float> buff( LENGTH_WORLD_VECTOR, 0);
    
    if( !(f_real_) )
    {
	sim_->get_world( &buff[0] );
	setWorld( buff );
    }

    pthread_mutex_lock( &(world_mutex) );

    /* Copiando todo o vetor de informações */
    world_vec = world;

    pthread_mutex_unlock( &(world_mutex) );
}


void
Monitor::setField( const vector<float> field_vec )
{    
    /* Atualizando o próprio campo */
    pthread_mutex_lock( &(field_mutex) );

    /* Copiando todo o vetor de informações */
    field = field_vec;

    pthread_mutex_unlock( &(field_mutex) );

}

void
Monitor::getField( vector<float> &field_vec )
{
    vector<float> buff(LENGTH_FIELD_VECTOR, 0);
    
    if( !(f_real_) )
    {
	sim_->get_field( &buff[0] );
	setField( buff );
    }

    /* Retirando informações do próprio campo */
    pthread_mutex_lock( &(field_mutex) );

    /* Copiando todo o vetor de informações */
    field_vec = field;

    pthread_mutex_unlock( &(field_mutex) );    
}
